#!/usr/bin/python

import sys
import os
import yaml
import _thread

import rospy
import rospkg
import actionlib
from actionlib_msgs.msg import *
from move_base_msgs.msg import MoveBaseActionResult
import common.msg
import common.srv
import swiftpro.msg
from std_msgs.msg import String
from swiftpro.msg import position
from vision_msgs.msg import Detection2DArray

import threading
import time

class HMTaskAction:
    def __init__(self) -> None:
        # 获取标定文件相关信息
        rospack = rospkg.RosPack()
        package_path = os.path.join(rospack.get_path('spark_hm_task_match'))
        cali_path = os.path.join(package_path, 'config', 'cali_config.yaml')
        items_path = os.path.join(package_path, 'config', 'items_config.yaml')
        try:
            with open(cali_path, "r") as f:
                self.cali_content = yaml.load(f.read())
            with open(items_path, "r") as f:
                items_content = yaml.load(f.read())
        except Exception as e:
            rospy.logerr(e)
            sys.exit(1)
        if isinstance(self.cali_content, type(None)):
            rospy.logerr("calibration file empty")
            sys.exit(1)
        if isinstance(items_content, type(None)):
            rospy.logerr("items file empty")
            sys.exit(1)

        # 创建控制spark直走的action客户端
        self.move_action_cli = actionlib.SimpleActionClient(
            'move_straight', common.msg.MoveStraightDistanceAction)
        self.move_action_cli.wait_for_server(
            timeout=rospy.Duration.from_sec(3))

        # 创建控制spark旋转的action客户端
        self.turn_action_cli = actionlib.SimpleActionClient(
            'turn_body', common.msg.TurnBodyDegreeAction)
        self.turn_action_cli.wait_for_server(
            timeout=rospy.Duration.from_sec(3))

        # 创建获取spark前后距离的service客户端
        rospy.wait_for_service('/get_distance')
        self.distance_srv = rospy.ServiceProxy(
            'get_distance', common.srv.GetFrontBackDistance)

        # 创建控制机械臂的topic发布者
        self.arm_position_pub = rospy.Publisher(
            "position_write_topic", swiftpro.msg.position, queue_size=1)
        self.arm_pump_pub = rospy.Publisher(
            "pump_topic", swiftpro.msg.status, queue_size=1)
        self.arm_status_pub = rospy.Publisher(
            "swiftpro_status_topic", swiftpro.msg.status, queue_size=1)

        # 创建导航地点的话题发布者
        self.local_goto_pub = rospy.Publisher(
            "mark_nav", String, queue_size=1)

        # 创建仓门打开关闭状态显示的topic发布者
        self.gate_pub = rospy.Publisher("hm_task_gate_cmd", String, queue_size=1)

        # 定义名称
        self.TABLE_1 = "t_1"
        self.TABLE_2 = "t_2"
        self.TABLE_3 = "t_3"
        self.WAREHOUSE_L = "warehouse_l"
        self.WAREHOUSE_M = "warehouse_m"
        self.WAREHOUSE_S = "warehouse_s"

    def local_goto(self, name):
        '''
        根据目标点名称,发布目标位置到MoveBase服务器, 根据返回状态进行判断
        @return: True 为成功到达, False 为失败
        '''

        # 发布目标位置
        self.local_goto_pub.publish("go "+name)

        # 设定1分钟的时间限制, 进行阻塞等待
        try:
            ret_status = rospy.wait_for_message(
                'move_base/result', MoveBaseActionResult, rospy.Duration(60)).status.status
        except Exception:
            rospy.logwarn("nav timeout!!!")
            ret_status = GoalStatus.ABORTED

        # 如果一分钟之内没有到达, 放弃目标
        if ret_status != GoalStatus.SUCCEEDED:
            rospy.Publisher("move_base/cancel", GoalID, queue_size=1).publish(
                GoalID(stamp=rospy.Time.from_sec(0.0), id=""))
            try:
                rospy.wait_for_message(
                    'move_base/result', MoveBaseActionResult, rospy.Duration(3))
            except Exception:
                rospy.logwarn("move_base result timeout. this is abnormal.")
            rospy.loginfo("========== Timed out achieving goal ==========")
            # rospy.sleep(2)
            return False
        else:
            rospy.loginfo("========== Goal succeeded ==========")
            # rospy.sleep(2) # 停稳
            return True



    def hm_screen(self, gate_cmd:bool):
        '''
        让显示屏显示特定文字
        @parm gate_cmd: True 显示 OPEN, False 显示 CLOSE
        @return: True 为成功, False 为失败
        '''
        if gate_cmd:
            self.gate_pub.publish(String("open")) # 第一次放置时需设置开发板为open
        else:
            self.gate_pub.publish(String("close")) # 最后一次放置时需设置开发板为close

        return True


    def arm_position_reset(self):
        '''
        校准机械臂的坐标系, 机械臂因碰撞导致坐标计算出问题时使用
        '''
        r1 = rospy.Rate(10)
        # r1.sleep()
        self.arm_status_pub.publish(swiftpro.msg.status(0))
        r1.sleep()
        self.arm_status_pub.publish(swiftpro.msg.status(1))
        r1.sleep()

    def arm_home(self, block=False):
        '''
        收起机械臂(无物品)
        '''
        pos = position()
        pos.x = 130
        pos.y = 0
        pos.z = 35
        rospy.loginfo("arm to home")
        self.arm_position_pub.publish(pos)
        if block:
            rospy.sleep(3)


    def arm_grasp_ready(self, block=False):
        '''
        移动机械臂到摄像头看不到的地方
        '''
        pos = position()
        pos.x = 50
        # pos.y = 130
        # pos.z = 130
        pos.y = 180
        pos.z = 160
        rospy.loginfo("arm to grasp ready pos")
        self.arm_position_pub.publish(pos)
        if block:
            rospy.sleep(3)

    def sow1(self):
        '''
        播种
        '''
        r1 = rospy.Rate(0.5)
        r2 = rospy.Rate(10)
        pos = position()

        # 默认放置位置
        pos.x = 200
        pos.y = 150
        pos.z = 100
        # rospy.loginfo("to drop pose")
        self.arm_position_pub.publish(pos)
        r1.sleep()

        # 放下
        pos.x = 200
        pos.y = 180
        pos.z = 10
        self.arm_position_pub.publish(pos)
        r1.sleep()

    def sow2(self):
        '''
        播种
        '''
        r1 = rospy.Rate(0.5)
        r2 = rospy.Rate(10)
        pos = position()

        # 默认放置位置
        pos.x = 200
        pos.y = -150
        pos.z = 100
        # rospy.loginfo("to drop pose")
        self.arm_position_pub.publish(pos)
        r1.sleep()

        # 放下
        pos.x = 200
        pos.y = -180
        pos.z = 10
        self.arm_position_pub.publish(pos)
        r1.sleep()

# 种植的线程，为了与前往路径点同时进行
def sow_thread_fun():
    act = HMTaskAction()
    msg_pub = rospy.Publisher('sow_progress', String)
    progress = "sowing"
    msg_pub.publish(progress)
    act.sow()
    act.arm_home()
    rospy.sleep(1)

# 根据不同的控制模式进行不同操作，分为auto和manual
def control_mode(mode):
    act = HMTaskAction()
    str_mode = str(mode) # mode转换为字符型变量
    msg_pub = rospy.Publisher('sow_progress', String)
    sta_pub = rospy.Publisher('state', String)

    if str.find(str_mode, str("manual_mode"))!= -1:  # 手动，定点插秧
        state = "manual"
        sta_pub.publish(state)
        rospy.loginfo("manual_mode")
        # 前面先学习四个地点
        # 等待开始执行任务的指令
        rospy.loginfo("wait for message on hm_task_match_start topic...")
        rospy.wait_for_message("hm_task_match_start", String)

        act.hm_screen(True)  # 第一次放置前需设置开发板为open

        act.local_goto("wp_1")
        progress = "sowing"
        msg_pub.publish(progress)  # 发布正在播种的信息

        # 创建一个线程来进行种植操作
        for i in range(5):
            # 直走一段距离
            act.move_action_cli.send_goal_and_wait(
                common.msg.MoveStraightDistanceGoal(
                    type=common.msg.MoveStraightDistanceGoal.TYPE_ODOM,
                    const_rot_vel=0.3,
                    move_distance=0.8,
                ),
                rospy.Duration.from_sec(5)  # 超过5s为超时
            )
            # 进行一次播种操作
            act.sow1()
            act.arm_home()
            rospy.sleep(1)
            times = (i + 1) * 10
            times = str(times)
            msg_pub.publish(times)

        progress = "first_sowed"  # 发送第一列已种植完的信息
        msg_pub.publish(progress)

        act.local_goto("wp_2")

        act.local_goto("wp_3")
        progress = "sowing"
        msg_pub.publish(progress)
        # 创建一个线程来进行种植操作
        for i in range(5):
            # 直走一段距离
            act.move_action_cli.send_goal_and_wait(
                common.msg.MoveStraightDistanceGoal(
                    type=common.msg.MoveStraightDistanceGoal.TYPE_ODOM,
                    const_rot_vel=0.3,
                    move_distance=0.8,
                ),
                rospy.Duration.from_sec(5)  # 超过5s为超时
            )
            # 进行一次播种操作
            act.sow2()
            act.arm_home()
            rospy.sleep(1)
            times = (i + 1) * 10
            times = str(times)
            msg_pub.publish(times)

        progress = "second_sowed"  # 发送第二列已种植完的信息
        msg_pub.publish(progress)

        act.local_goto("wp_4")

        act.hm_screen(False)  # 最后一次放置后需设置开发板为close
    
    else:
        act.move_action_cli.send_goal_and_wait(
                common.msg.MoveStraightDistanceGoal(
                    type=common.msg.MoveStraightDistanceGoal.TYPE_ODOM,
                    const_rot_vel=0.1,
                    move_distance=0.2,
                ),
                rospy.Duration.from_sec(5)  # 超过5s为超时
            )
        rospy.loginfo(type(str_mode))
        rospy.loginfo(str_mode)

def sowing(sow_type):
    act = HMTaskAction()
    str_sowtype = str(sow_type) # mode转换为字符型变量
    if str.find(str_sowtype, str("sow1"))!= -1:  # 左侧插秧
        act.sow1()
        act.arm_home()
    if str.find(str_sowtype, str("sow2"))!= -1:  # 右侧插秧
        act.sow2()
        act.arm_home()


def main(threadName, delay):
    ret = False

    item_type = 0
    act = HMTaskAction()
    rospy.sleep(1)
    act.arm_grasp_ready()  # 移动机械臂到其他地方

    msg_pub = rospy.Publisher('sow_progress', String)


    act.arm_home()  # 移动机械臂到初始位置

    rospy.Subscriber('control_mode', String, control_mode)
    # rospy.Subscriber('sow_topic', String, sowing)



if __name__ == '__main__':
    rospy.init_node('spark_runtask', anonymous=True)


    _thread.start_new_thread(main, ("Thread-1", 2, ))
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print("Shutting down!!!")
